﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.Util;
using Emgu.CV.Structure;

namespace Autofocus
{
    static class Phase
    {
        public static void Calc_Phase_Correlation(Image<Gray, float> img1, Image<Gray, float> img2, ref Int32 d_X, ref Int32 d_Y)
        {
            // Check that 2 images are the same size
            if (img1.Size != img2.Size)
            {
                MessageBox.Show("Two images are not the same size!");
                return;
            }

            // Obtain DFT for first image
            // Set up image with border
            Int32 extended_image_1_width = CvInvoke.cvGetOptimalDFTSize(System.Convert.ToInt32((double)img1.Size.Width * 1.25));
            Int32 extended_image_1_height = CvInvoke.cvGetOptimalDFTSize(System.Convert.ToInt32((double)img1.Size.Height * 1.25));
            Image<Gray, float> extended_image_1 = new Image<Gray, float>(extended_image_1_width, extended_image_1_height);

            Int32 x_offset = System.Convert.ToInt32((double)(extended_image_1.Size.Width - img1.Size.Width) / 2);
            Int32 y_offset = System.Convert.ToInt32((double)(extended_image_1.Size.Height - img1.Size.Height) / 2);
            CvInvoke.cvCopyMakeBorder(img1, extended_image_1, new Point(x_offset, y_offset), Emgu.CV.CvEnum.BORDER_TYPE.REFLECT, new MCvScalar(0));
            Fade_Border(ref extended_image_1, img1, x_offset, y_offset);
            //imageBox1.Image = extended_image_1;

            // Set up two channel matrix. First channel is real image data. Second channel is imaginary data (=0)
            Matrix<float> dft1_in = new Matrix<float>(extended_image_1.Rows, extended_image_1.Cols, 2);
            CvInvoke.cvMerge(extended_image_1, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, dft1_in);

            // Set up matrix to hold DFT and perform DFT
            Matrix<float> dft1 = new Matrix<float>(dft1_in.Rows, dft1_in.Cols, 2);
            CvInvoke.cvDFT(dft1_in, dft1, Emgu.CV.CvEnum.CV_DXT.CV_DXT_FORWARD, 0);


            // Obtain DFT for second image
            Image<Gray, float> extended_image_2 = new Image<Gray, float>(extended_image_1.Size);
            CvInvoke.cvCopyMakeBorder(img2, extended_image_2, new Point(x_offset, y_offset), Emgu.CV.CvEnum.BORDER_TYPE.REFLECT, new MCvScalar(0));
            Fade_Border(ref extended_image_2, img2, x_offset, y_offset);
            //imageBox2.Image = extended_image_2;

            // Set up two channel matrix. First channel is real image data. Second channel is imaginary data (=0)
            Matrix<float> dft2_in = new Matrix<float>(extended_image_2.Rows, extended_image_2.Cols, 2);
            CvInvoke.cvMerge(extended_image_2, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, dft2_in);

            // Set up matrix to hold DFT and perform DFT
            Matrix<float> dft2 = new Matrix<float>(dft2_in.Rows, dft2_in.Cols, 2);
            CvInvoke.cvDFT(dft2_in, dft2, Emgu.CV.CvEnum.CV_DXT.CV_DXT_FORWARD, 0);



            // Obtain the cross power spectrum
            Matrix<float> cps = new Matrix<float>(dft1.Rows, dft1.Cols, 2);
            Matrix<float> norms = new Matrix<float>(dft1.Rows, dft1.Cols, 1);
            CvInvoke.cvMulSpectrums(dft1, dft2, cps, Emgu.CV.CvEnum.MUL_SPECTRUMS_TYPE.CV_DXT_MUL_CONJ);

            // Split the DFT in to real and im parts
            Matrix<float> outReal = new Matrix<float>(dft1.Size);
            Matrix<float> outIm = new Matrix<float>(dft1.Size);
            Matrix<float> outReal_squared = new Matrix<float>(dft1.Size);
            Matrix<float> outIm_squared = new Matrix<float>(dft1.Size);
            Matrix<float> sum_of_squares = new Matrix<float>(dft1.Size);
            CvInvoke.cvSplit(cps, outReal, outIm, IntPtr.Zero, IntPtr.Zero);

            // Per-element normalise by product
            CvInvoke.cvPow(outReal, outReal_squared, 2);
            CvInvoke.cvPow(outIm, outIm_squared, 2);
            sum_of_squares = outReal_squared.Add(outIm_squared);
            CvInvoke.cvPow(sum_of_squares, norms, 0.5);

            CvInvoke.cvDiv(outReal, norms, outReal, 1);
            CvInvoke.cvDiv(outIm, norms, outIm, 1);

            // Merge real and im parts
            CvInvoke.cvMerge(outReal, outIm, IntPtr.Zero, IntPtr.Zero, cps);

            //Show The Data       
            //CvInvoke.cvShowImage("Real", outReal);
            //CvInvoke.cvShowImage("Imaginary ", outIm);


            // Obtain the inverse DFT of the cross power spectrum
            Image<Gray, float> poc = new Image<Gray, float>(dft1.Size);
            CvInvoke.cvDFT(cps, poc, Emgu.CV.CvEnum.CV_DXT.CV_DXT_INV_SCALE, 0);

            // Find the peak in the phase correlation matrix
            //double minVal = 0;
            //double maxVal = 0;
            //Point minLoc = Point.Empty;
            //Point maxLoc = Point.Empty;
            //Double best_cross_corr;

            //imageBox3.Image = poc;
            //CvInvoke.cvMinMaxLoc(poc, ref minVal, ref maxVal, ref minLoc, ref maxLoc, IntPtr.Zero);

            // Find the n highest local maxima in 3x3 neighbourhood
            Int32 no_of_maxima = 5;
            float[] value_of_maxima = new float[no_of_maxima];
            Point[] location_of_maxima = new Point[no_of_maxima];
            for (int i = 0; i < no_of_maxima; i++)
            {
                value_of_maxima[i] = 0;
            }

            Image<Gray, float> poc_dilated = new Image<Gray, float>(poc.Size);
            poc_dilated = poc.Dilate(1);
            for (int x = 0; x < poc.Size.Width; x++)
            {
                for (int y = 0; y < poc.Size.Height; y++)
                {
                    if ((poc.Data[y, x, 0] > 0) && (poc.Data[y, x, 0] == poc_dilated.Data[y, x, 0]))
                    {
                        // this point is a local maximum
                        // compare it to the current list of maxima
                        for (int n = 0; n < no_of_maxima; n++)
                        {
                            if (poc.Data[y, x, 0] > value_of_maxima[n])
                            {
                                for (int m = no_of_maxima - 1; m > n; m--)
                                {
                                    value_of_maxima[m] = value_of_maxima[m - 1];
                                    location_of_maxima[m] = location_of_maxima[m - 1];
                                }
                                value_of_maxima[n] = poc.Data[y, x, 0];
                                location_of_maxima[n].X = x;
                                location_of_maxima[n].Y = y;
                                break;
                            }
                        }
                    }
                }
            }

            // Fourier series is periodic so need to test different
            // translations using cross-correlation on overlapping areas
            Point[] location_of_all_maxima = new Point[no_of_maxima * 4];
            float[] value_of_all_cross_corr = new float[no_of_maxima * 4];
            float best_cross_corr = 0;
            Point best_maxima = new Point(0, 0);

            // Expand list of maxima so that it contains all shifts
            for (int i = 0; i < no_of_maxima; i++)
            {
                location_of_all_maxima[i * 4 + 0].X = -location_of_maxima[i].X;
                location_of_all_maxima[i * 4 + 0].Y = -location_of_maxima[i].Y;

                // X positive, Y negative
                if (location_of_maxima[i].X != 0)
                    location_of_all_maxima[i * 4 + 1].X = extended_image_1.Size.Width - location_of_maxima[i].X;
                else
                    location_of_all_maxima[i * 4 + 1].X = 0;
                location_of_all_maxima[i * 4 + 1].Y = -location_of_maxima[i].Y;

                // X negative, Y positive
                location_of_all_maxima[i * 4 + 2].X = -location_of_maxima[i].X;
                if (location_of_maxima[i].Y != 0)
                    location_of_all_maxima[i * 4 + 2].Y = extended_image_1.Size.Height - location_of_maxima[i].Y;
                else
                    location_of_all_maxima[i * 4 + 2].Y = 0;

                // Both negative
                if (location_of_maxima[i].X != 0)
                    location_of_all_maxima[i * 4 + 3].X = extended_image_1.Size.Width - location_of_maxima[i].X;
                else
                    location_of_all_maxima[i * 4 + 3].X = 0;
                if (location_of_maxima[i].Y != 0)
                    location_of_all_maxima[i * 4 + 3].Y = extended_image_1.Size.Height - location_of_maxima[i].Y;
                else
                    location_of_all_maxima[i * 4 + 3].Y = 0;
            }


            for (int i = 0; i < no_of_maxima * 4; i++)
            {
                value_of_all_cross_corr[i] = Cross_Corr(img1, img2, location_of_all_maxima[i].X, location_of_all_maxima[i].Y);
                if (value_of_all_cross_corr[i] > best_cross_corr)
                {
                    best_cross_corr = value_of_all_cross_corr[i];
                    best_maxima = location_of_all_maxima[i];
                }
            }

            d_X = best_maxima.X;
            d_Y = best_maxima.Y;
        }

        private static void Fade_Border(ref Image<Gray, float> extended_image, Image<Gray, float> img1, Int32 x_offset, Int32 y_offset)
        {
            // Fade out reflected regions
            Image<Gray, float> intensity_mask = new Image<Gray, float>(extended_image.Size);

            float count = 0;
            for (int x = 0; x < x_offset; x++)
            {
                for (int y = 0; y < intensity_mask.Size.Height; y++)
                {
                    intensity_mask.Data[y, x, 0] = System.Convert.ToSingle(-5.5 * (1.0 - count / x_offset));
                }
                count++;
            }

            count = 0;
            for (int x = x_offset + img1.Size.Width; x < intensity_mask.Size.Width; x++)
            {
                for (int y = 0; y < intensity_mask.Size.Height; y++)
                {
                    intensity_mask.Data[y, x, 0] = System.Convert.ToSingle(-5.5 * count / x_offset);
                }
                count++;
            }

            count = 0;
            for (int y = 0; y < y_offset; y++)
            {
                for (int x = 0; x < intensity_mask.Size.Width; x++)
                {
                    intensity_mask.Data[y, x, 0] += System.Convert.ToSingle(-5.5 * (1.0 - count / y_offset));
                }
                count++;
            }

            count = 0;
            for (int y = y_offset + img1.Size.Height; y < intensity_mask.Size.Height; y++)
            {
                for (int x = 0; x < intensity_mask.Size.Width; x++)
                {
                    intensity_mask.Data[y, x, 0] += System.Convert.ToSingle(-5.5 * count / y_offset);
                }
                count++;
            }

            intensity_mask = intensity_mask.Exp();
            extended_image = extended_image.Mul(intensity_mask);
        }

        private static float Cross_Corr(Image<Gray, float> img_1, Image<Gray, float> img_2, Int32 delta_x, Int32 delta_y)
        {
            // This function returns the cross-correlation of the two images with the shift specified in maxLoc
            float cross_corr = 0;
            Rectangle img1_subrect;
            Rectangle img2_subrect;
            Image<Gray, float> img_cross_corr;

            // Check that these is some overlap
            if (Math.Abs(delta_x) < img_1.Size.Width && Math.Abs(delta_y) < img_1.Size.Height)
            {
                img1_subrect = new Rectangle(Math.Max(-delta_x, 0), Math.Max(-delta_y, 0), img_1.Size.Width - Math.Abs(delta_x), img_1.Size.Height - Math.Abs(delta_y));
                img2_subrect = new Rectangle(Math.Max(delta_x, 0), Math.Max(delta_y, 0), img_1.Size.Width - Math.Abs(delta_x), img_1.Size.Height - Math.Abs(delta_y));
                img_1.ROI = img1_subrect;
                img_2.ROI = img2_subrect;
                img_cross_corr = img_1.MatchTemplate(img_2, Emgu.CV.CvEnum.TM_TYPE.CV_TM_CCORR_NORMED);
                img_1.ROI = System.Drawing.Rectangle.Empty;
                img_2.ROI = System.Drawing.Rectangle.Empty;

                cross_corr = img_cross_corr.Data[0, 0, 0];
                img_cross_corr.Dispose(); // mcc 11/04
            }

            return cross_corr;
        }

    }
}
